/****************************************************************************/
/*	include files
*****************************************************************************/
#include "include.h"

/****************************************************************************/
/*	Local variable  definitions
*****************************************************************************/

/*******************************************
*clark,park,res_clark,re_park
********************************************/



volatile foc_struct Foc_lib = {0};
volatile Union_Flag Flag = {0};
volatile MC_MotorStates MOTOR_STATE = MC_INIT ;
volatile MC_MotorStates TARGET_STATE = MC_INIT;
volatile s16 AngleCal = DEG_30;
volatile u8 slopecnt = 0;
volatile u8 AddStep = ADD_STEP_VAL;

void ResetOtherParameterFun(void)
{
	AddStep = ADD_STEP_VAL;
}

void AddVqControlFun(void)
{
	if(AddStep < 20)
	{
		AddStep++;
	}
}


inline void MotorSquareRunFun(void)
{
	if(Hall.DirFlag == CW)
	{
		Hall.Angle = Hall.HallAngle + AngleCal;
		Foc_lib.Angle = Hall.Angle + Foc_lib.HeadAngle;
	}
	else if(Hall.DirFlag == CCW)
	{
		Hall.Angle = Hall.HallAngle  - AngleCal;
		Foc_lib.Angle = Hall.Angle - Foc_lib.HeadAngle;
	}
	SinCosCalFun(Foc_lib.Angle);
//	Park();
	PIDIQResumeFun(Foc_lib.Vq);
	if(Foc_lib.Vq < Foc_lib.ClosedTargetVq)
	{
		if(++slopecnt >= 1)
		{
			slopecnt = 0;
			Foc_lib.Vq = Foc_lib.Vq + ADD_STEP_VAL;
			if(Foc_lib.Vq > Foc_lib.ClosedTargetVq)
			{
				Foc_lib.Vq = Foc_lib.ClosedTargetVq;
			}
		}
	}
	else if(Foc_lib.Vq > Foc_lib.ClosedTargetVq)
	{
		if(++slopecnt >= 1)
		{
			slopecnt = 0;
			Foc_lib.Vq = Foc_lib.Vq - 30;
			if(Foc_lib.Vq < 0)
			{
				Foc_lib.Vq = 0;
			}
		}
	}
	else
	{
		slopecnt = 0;
	}

	if(Foc_lib.Vq > Foc_lib.VqLimit)
	{
		Foc_lib.Vq = Foc_lib.VqLimit;
	}
	else if(Foc_lib.Vq < Foc_lib.MinVq)
	{
		Foc_lib.Vq = Foc_lib.MinVq;
	}
}



inline void MotorFocRunFun(void)
{
	FocHallAngleAdjustFun();
	if(Hall.DirFlag == CW)
	{
		Hall.Angle = Hall.Angle + Hall.HallOmegaPeroid + Hall.StepTheta;
		Foc_lib.Angle = Hall.Angle + Foc_lib.HeadAngle;
	}
	else if(Hall.DirFlag == CCW)
	{
		Hall.Angle = Hall.Angle - Hall.HallOmegaPeroid - Hall.StepTheta;
		Foc_lib.Angle = Hall.Angle - Foc_lib.HeadAngle;
	}

	SinCosCalFun(Foc_lib.Angle);
	Park();
	Foc_lib.Vd = PidCalFun(&PID_ID,0,Foc_lib.Id);
	if(CPQ_STATE == RUN_PRESS)
	{
		Foc_lib.Vq = PidCalFun(&PID_IQ,Foc_lib.TargetIq,Foc_lib.Iq);
	}
	else 
	{
		Foc_lib.Vq = PidCalFun(&PID_IQ,40000,Foc_lib.IqPress);
	}
	if(Foc_lib.Vq > Foc_lib.ClosedTargetVq)
	{
		Foc_lib.Vq = Foc_lib.ClosedTargetVq;
	}

	if(Foc_lib.Vq > Foc_lib.VqLimit)
	{
		Foc_lib.Vq = Foc_lib.VqLimit;
	}
	else if(Foc_lib.Vq < Foc_lib.MinVq)
	{
		Foc_lib.Vq = Foc_lib.MinVq;
	}
}






inline void ReadyStopFun(void)
{
	if(TARGET_STATE == MC_INIT)
	{
		MOTOR_STATE = MC_BREAK;
//		MOTOR_STATE = MC_STOP;
	}
}


/*******PWM 50us,1000*50us = 50ms;*******/
/******Charge cap*********/
void ChargeStateFun(void)
{
	if(Foc_lib.ChargeCnt < 50)
	{
		Foc_lib.ChargeCnt++;
		Foc_lib.Vq = 0;
		Foc_lib.Vd = 0;
	}
	else
	{
		Foc_lib.ChargeCnt = 0;
		MOTOR_STATE = MC_SQUARE;
	}
}




/*******PWM 50us,1000*50us = 50ms;*******/
/******Charge cap*********/
void BrakeStateFun(void)
{
	if(Foc_lib.BrakeCnt < 4000)
	{
		Foc_lib.BrakeCnt++;
		Foc_lib.Vq = 0;
		Foc_lib.Vd = 0;

	}
	else
	{
		MOTOR_STATE = MC_STOP;
		Foc_lib.BrakeCnt = 0;
	}
}

void FOCTestFun(void)
{
	if(GPIO3->DO_f.P6 == 0)
	{
		GPIO3->DO_f.P6 = 1;
	}
	else
	{
		GPIO3->DO_f.P6 = 0;
	}	
}

inline void MotorRunFun(void)
{
	GetShunt2PhaseCurrentFun();
	GetIbusCurrentFun();
	Clarke();
	NormalHallRunFun();//when get new hall value, than calculate the omega;
	switch(MOTOR_STATE)
	{
		case MC_INIT:
		{
			break;
		}
		case MC_CHARGE:
		{
			BrigeOutputOnFun();
			MotorBrakeFun();
			ChargeStateFun();
			ReadyStopFun();
			break;
		}
		case MC_SQUARE:
		{
			AddVqControlFun();
			MotorSquareRunFun();
			ResPark();
			SvpwmCalFun();
			ReadyStopFun();
			break;
		}  
		
		case MC_FOC:
		{
			AddVqControlFun();
			MotorFocRunFun();
			ResPark();
			SvpwmCalFun();
			ReadyStopFun();
			break;
		}   
		case MC_WINDMILL:
		{
//			WindmillGetHallAngleFun();
			return;
		}
		case MC_BREAK:
		{
			BrakeStateFun();
			MotorBrakeFun();
			return;
		}
		case MC_STOP:
		{
			MOTOR_STATE = MC_INIT;
			BrigeOutputOffFun();
			FOCParameterInitFun();
			break;
		}
		default :
			break;
	}	
}








